// Copyright 2022 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "mjpc/tasks/fingers/fingers.h"

#include <string>

#include <absl/random/random.h>
#include <mujoco/mujoco.h>
#include "mjpc/task.h"
#include "mjpc/utilities.h"

namespace mjpc {
std::string Fingers::XmlPath() const {
  return GetModelPath("fingers/task.xml");
}
std::string Fingers::Name() const { return "FreeFingers"; }

void Fingers::ResidualFn::Residual(const mjModel* model, const mjData* data,
                     double* residual) const {
  int counter = 0;

  // reach
  double* finger_a = SensorByName(model, data, "finger_a");
  double* box = SensorByName(model, data, "object");
  mju_sub3(residual + counter, finger_a, box);
  counter += 3;
  double* finger_b = SensorByName(model, data, "finger_b");
  mju_sub3(residual + counter, finger_b, box);
  counter += 3;

  // bring
  for (int i=0; i < 3; i++) {
    double* object = SensorByName(model, data, std::to_string(i).c_str());
    double* target = SensorByName(model, data,
                                        (std::to_string(i) + "t").c_str());
    residual[counter++] = mju_dist3(object, target);
  }

  // control
  for (int i=0; i < model->nu; i++) {
    residual[counter++] = data->ctrl[i];
  }

  CheckSensorDim(model, counter);
}
}  // namespace mjpc
